/* $Id: camera_info_manager.cpp 28518 2010-03-31 15:47:24Z pbeeson $ */

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010 Jack O'Quin
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the author nor other contributors may be
*     used to endorse or promote products derived from this software
*     without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <string>
#include <ros/ros.h>
#include <camera_calibration_parsers/parse.h>

#include "camera_info_manager.h"

using namespace camera_calibration_parsers;

/** @file

    @brief CameraInfo Manager implementation

    Provides CameraInfo, handles the SetCameraInfo service requests,
    saves and restores sensor_msgs/CameraInfo data.

@par Advertises

 - @b set_camera_info service (sensor_msgs/SetCameraInfo) for
   calibration information

@par Parameters

 - @b camera_info_manager/url : @b [string] URL for getting and saving
   calibration data.  Example URL strings:

   - "file:///full/path/to/local/file.yaml"
   - "flash:///1" (not yet implemented)

    @author Jack O'Quin
 */


/** Constructor
 *
 * @param nh private node handle for driver
 * @param url default Uniform Resource Locator to get and save the data
 *        unless ~/camera_info_manager/url ROS parameter is set (default:
 *        no initial data).
 */
CameraInfoManager::CameraInfoManager(ros::NodeHandle nh, const std::string &url)
{
  nh_ = nh;                             // save copy of node handle

  // set default camera name to base node name of driver
  std::string node_name = ros::this_node::getName();
  size_t basepos = node_name.find_last_of("/");
  camera_name_ = node_name.substr(basepos+1);

  // look up URL in parameter server, use constructor argument as a default
  nh_.param("camera_info_manager/url", url_, url);

  // Get camera calibration data (if any).
  getCalibration(url_);

  // register callback for camera calibration service request
  info_service_ = nh_.advertiseService("set_camera_info",
                                       &CameraInfoManager::setCameraInfo,
                                       this);
}

/** get CameraInfo calibration data (if any)
 *
 * @param url Uniform Resource Locator for the data.
 * @return true, if successful
 */
bool CameraInfoManager::getCalibration(const std::string &url)
{
  bool success = false;                 // return value

  if (url == "")
    {
      ROS_WARN("no camera calibration source");
      return false;
    }

  ROS_INFO_STREAM("camera calibration URL: " << url);

  if (url.substr(0, 8) == "file:///")
    {
      success = getCalibrationFile(url.substr(7));
    }
  else if (url.substr(0, 9) == "flash:///")
    {
      ROS_WARN("CameraInfoManager reading from flash not implemented yet");
    }
  else
    {
      ROS_ERROR_STREAM("Invalid camera calibration URL: " << url);
    }

  return success;
}

/** get CameraInfo calibration data from a file
 *
 * @param filename contains CameraInfo to save
 * @return true, if successful
 */
bool CameraInfoManager::getCalibrationFile(const std::string &filename)
{
  bool success = false;

  ROS_INFO_STREAM("reading camera calibration from " << filename);
  std::string cam_name;
  sensor_msgs::CameraInfo cam_info;
  if (readCalibration(filename, cam_name, cam_info))
    {
      camera_name_ = cam_name;
      cam_info_ = cam_info;
      success = true;
    }
  else
    {
      ROS_WARN_STREAM("Camera calibration file " << filename << " not found.");
    }

  return success;
}
  
/** save CameraInfo calibration data
 *
 * @param new_info contains CameraInfo to save
 * @param url points to storage location (if empty, use
 *            "file:///tmp/calibration_<camera_name>.yaml")
 * @return true, if successful
 */
bool
CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info,
                                   const std::string &url)
{
  bool success = false;

  if (url == "")
    {
      // store using default file name
      std::string filename = "/tmp/calibration_" + camera_name_ + ".yaml";
      success = saveCalibrationFile(new_info, filename);
    }
  else if (url.substr(0, 8) == "file:///")
    {
      success = saveCalibrationFile(new_info, url.substr(7));
    }
  else if (url.substr(0, 9) == "flash:///")
    {
      ROS_WARN("CameraInfoManager writing to flash not implemented yet");
    }
  else
    {
      ROS_ERROR_STREAM("CameraInfoManager invalid url: "
                       << url << " (ignored)");
      success = saveCalibration(new_info, "");
    }

  return success;
}
  
/** save CameraInfo calibration data to a file
 *
 * @param new_info contains CameraInfo to save
 * @param filename is local file to store data
 * @return true, if successful
 */
bool
CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
                                       const std::string &filename)
{
  ROS_INFO_STREAM("writing calibration data to " << filename);
  return writeCalibration(filename, camera_name_, new_info);
}

/** Callback for SetCameraInfo request
 *
 * Always updates local copy, even if save fails.
 *
 * @param req SetCameraInfo request message
 * @param rsp SetCameraInfo response message
 * @return true if message handled
 */
bool 
CameraInfoManager::setCameraInfo(sensor_msgs::SetCameraInfo::Request &req,
                                 sensor_msgs::SetCameraInfo::Response &rsp)
{
  // save calibration data internally, regardless of success
  cam_info_ = req.camera_info;

  if (!nh_.ok())
    {
      ROS_ERROR("set_camera_info service called, but driver not running.");
      rsp.status_message = "Camera driver not running.";
      rsp.success = false;
      return false;
    }

  // TODO: It would be nice to check that the newly requested
  // calibration is compatible with the currently configured
  // video_mode, but that information is not always available here.

  // re-fetch URL from parameter server, in case it changed
  std::string new_url;
  if (nh_.getParam("camera_info_manager/url", new_url))
    url_ = new_url;

  rsp.success = saveCalibration(cam_info_, url_);
  if (!rsp.success)
    rsp.status_message = "Error storing camera calibration.";

  return true;
}
